#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S3,     seeker,         sensorHiTechnicIRSeeker600)
#pragma config(Motor,  mtr_S1_C1_1,     Left,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Right,         tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    flipper,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

/*void IRValue4()
{
	while (nMotorEncoder[Left] > -1440)
	{
		motor[Left] = -50;
		motor[Right] = -50;
	}
	motor[Left] = 0;
	motor[Right] = 0;
	nMotorEncoder[Left] = 0;

	while (nMotorEncoder[Left] < 720)
	{
		motor[Left] = 50;
		motor[Right] = -50;
	}
	motor[Left] = 0;
	motor[Right] = 0;
	nMotorEncoder[Left] = 0;

	while (SensorValue[seeker] < 7)
	{
		motor[Left] = -50;
		motor[Right] = -50;
	}
	motor[Left] = 0;
	motor[Right] = 0;

	while (SensorValue[seeker] != 5)
	{
		motor[Left] = -50;
		motor[Right] = 50;
	}
	motor[Left] = 0;
	motor[Right] = 0;

	servo[flipper] = 128;
}*/

void IRValue6()
{
	while (nMotorEncoder[Left] < -1440)
	{
		motor[Left] = -50;
		motor[Right] = -50;
	}
	motor[Left] = 0;
	motor[Right] = 0;
	nMotorEncoder[Left] = 0;

	while (nMotorEncoder[Left] > -700)
	{
		motor[Left] = -50;
		motor[Right] = 50;
	}
	motor[Left] = 0;
	motor[Right] = 0;
	nMotorEncoder[Left] = 0;

	while (SensorValue[seeker] > 3)
	{
		motor[Left] = -50;
		motor[Right] = -50;

	motor[Left] = 0;
	motor[Right] = 0;

	servo[flipper] = 120;
}

task main()
{
	waitForStart();
	int beaconPos = SensorValue[seeker];
	nMotorEncoder[Left] = 0;
	while(true)
	{
		if (beaconPos < 5)
		{
			//IRValue4();
			while (nMotorEncoder[Left] > -1440)
			{
				motor[Left] = -50;
				motor[Right] = -50;
			}
			motor[Left] = 0;
			motor[Right] = 0;
			nMotorEncoder[Left] = 0;

			while (nMotorEncoder[Left] < 220)
			{
				motor[Left] = 50;
				motor[Right] = -50;
			}
			motor[Left] = 0;
			motor[Right] = 0;
			nMotorEncoder[Left] = 0;

			while (SensorValue[seeker] > 3)
			{
				motor[Left] = -50;
				motor[Right] = -50;
			}
			motor[Left] = 0;
			motor[Right] = 0;

			while (SensorValue[seeker] < 5)
			{
				motor[Left] = -35;
				motor[Right] = 35;
			}
			motor[Left] = 0;
			motor[Right] = 0;

			servo[flipper] = 128;
		}
		else if (beaconPos > 5)
		{
			IRValue6();
		}
	}



}
